 // 初始化ODrive控制器
void initializeODrive() {
  // 设置轴0和轴1进入闭环控制模式
  sendODriveCommand("w axis0.requested_state 8");
  sendODriveCommand("w axis1.requested_state 8");
  delay(1000);

  // 设置控制模式为速度控制
  sendODriveCommand("w axis0.controller.config.control_mode 2");
  sendODriveCommand("w axis1.controller.config.control_mode 2");
  delay(1000);
  
  // 初始速度为0
  setODriveVelocity(0, 0);
  Serial.println("Odrive初始化成功");
}

//发送命令到odrive
void sendODriveCommand(String command) {
  ODRIVE_SERIAL.println(command);
  Serial.print("发送命令到ODrive: ");
  Serial.println(command);
}

//发送速度命令到odrive
void setODriveVelocity(float speed0, float speed1) {
  String cmd0 = "w axis0.controller.input_vel " + String(-speed0);
  String cmd1 = "w axis1.controller.input_vel " + String(speed1);
  sendODriveCommand(cmd0);
  sendODriveCommand(cmd1);
}

// 实现向前向后走固定距离行驶
void MoveDistance(int direction, int go_time,float speed_set) {
  // 设置行驶速度（根据方向决定正负）
  float speed = (direction == D_FRONT) ? speed_set : -speed_set; // 假设1.0为正向速度，-1.0为反向速度
  
  // 启动电机
  setODriveVelocity(speed, speed);
  
  // 记录开始时间
  unsigned long start_time = millis();
  
  // 持续运行直到达到指定的时间
  while (millis() - start_time < go_time) {
    // 可以在这里添加其他逻辑，如检查传感器等
    delay(10); // 小延迟以减少CPU负载
  }
  
  // 停止电机
  setODriveVelocity(0, 0);
  
  Serial.print("已完成");
  Serial.print(direction == D_FRONT ? "前进" : "后退");
  Serial.print("移动，持续时间：");
  Serial.print(go_time);
  Serial.println("ms");
}

// 实现固定角度旋转
void Angle_of_rotation(int direction, int time_to_angle) {
  // 根据方向设置左右轮速度（相反方向实现转向）
  float leftSpeed = (direction == D_RIGHT) ? 1.0 : -1.0;
  float rightSpeed = -leftSpeed;  // 另一侧轮子反向
  
  Serial.print("开始");
  Serial.print(direction == D_RIGHT ? "右转" : "左转");
  Serial.print("，目标时间: ");
  Serial.print(time_to_angle);
  Serial.println("ms");
  
  // 启动电机
  setODriveVelocity(leftSpeed, rightSpeed);
  
  // 记录开始时间
  unsigned long start_time = millis();
  
  // 持续运行直到达到指定的时间
  while (millis() - start_time < time_to_angle) {
    // 可以在这里添加其他逻辑，如检查传感器等
    delay(10); // 小延迟以减少CPU负载
  }
  
  // 停止电机
  setODriveVelocity(0, 0);
  
  Serial.println("旋转完成");
}